#include "ScenePrecompiled.hxx"
#include "SceneTransformationBase.hxx"

Eifersucht::TransformationBase::TransformationBase() : 
	GeometricTranslationCache(nullptr),
	GeometricRotationCache(nullptr),
	GeometricScaleCache(nullptr),
	LclTranslationCache(nullptr),
	LclRotationCache(nullptr),
	LclScaleCache(nullptr)
{
}

Eifersucht::TransformationBase::~TransformationBase()
{
	_Delete_pointer( GeometricTranslationCache );
	_Delete_pointer( GeometricRotationCache );
	_Delete_pointer( GeometricScaleCache );
	_Delete_pointer( LclTranslationCache );
	_Delete_pointer( LclRotationCache );
	_Delete_pointer( LclScaleCache );
}

Eifersucht::TransformationBase::PropertyCache::PropertyCache()
{
	//for (auto pChannel : Channels) pChannel = nullptr;
	Reign::Standard::ZeroValueArray(Channels, ARRAYSIZE(Channels));
}

Eifersucht::TransformationBase::PropertyCache::~PropertyCache()
{
	for (auto pChannel : Channels) _Delete_pointer(pChannel);
	//Reign::Standard::ZeroValueArray(Channels, ARRAYSIZE(Channels));
}

Eifersucht::TransformationBase::PropertyCache::ChannelCache::ChannelCache()
{
	Curves.clear();
}

Eifersucht::TransformationBase::PropertyCache::ChannelCache::~ChannelCache()
{
	for (auto pCurve : Curves) _Delete_pointer(pCurve.second);
	Curves.clear();
}

Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::CurveCache()
{
	Knots.clear();
}

Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::~CurveCache()
{
	for (auto pKnot : Knots) _Delete_pointer(pKnot);
	Knots.clear();
}

Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::Joint::Joint() : 
	Point(), Meets(), Leaves() //, Weights(0.333f), Slopes(-0.f, 0.f)
{
	PrevPtr = nullptr;
	NextPtr = nullptr;
}

Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::Joint::~Joint()
{
	PrevPtr = nullptr;
	NextPtr = nullptr;
}

void Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::Joint::UpdateBoth(
	_Inout_ JointPtr knot, 
	_In_ Reign::Vector2 const &weights, 
	_In_ Reign::Vector2 const &slopes
	)
{
	UpdateLeft(knot, weights.x, slopes.x);
	UpdateRight(knot, weights.y, slopes.y);
}

void Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::Joint::UpdateLeft( 
	_Inout_ JointPtr thisKnot, 
	_In_ float weight, 
	_In_ float slope
	)
{
	//thisKnot->LeftWeight(weight);
	//thisKnot->LeftSlope(slope);

	if (thisKnot->PrevPtr != nullptr)
	{
		auto const dt = thisKnot->Time() - thisKnot->PrevPtr->Time();
		auto const wdt = dt * weight;
		thisKnot->Meets.y =  slope * wdt;
		thisKnot->Meets.x = wdt;
	}

	/*thisKnot->Meets.y = slope > 0.0f ? 
		abs(slope) * wdt * -1.0f : 
		abs(slope) * wdt;*/
}

void Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::Joint::UpdateRight( 
	_Inout_ JointPtr thisKnot, 
	_In_ float weight, 
	_In_ float slope
	)
{
	//thisKnot->RightWeight(weight);
	//thisKnot->RightSlope(slope);

	if (thisKnot->NextPtr != nullptr)
	{
		auto const dt = thisKnot->NextPtr->Time() - thisKnot->Time();
		auto const wdt = dt * weight;
		thisKnot->Leaves.y =  slope * wdt;
		thisKnot->Leaves.x = wdt;
	}

	/*thisKnot->Leaves.y = slope > 0.0f ? 
		abs(slope) * wdt * -1.0f : 
		abs(slope) * wdt;*/
}

void Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::ReconnectKnots()
{
	JointCollection::const_iterator 
		fst = Knots.begin(), // + 0,
		lst = Knots.end() - 1; // + 0;
	JointCollection::iterator 
		prv = Knots.begin(), // + 0,
		crr = Knots.begin() + 1;
	(*lst)->NextPtr = nullptr; 
	(*fst)->Ref = this;
	(*lst)->Ref = this;
	lst++;

	while (crr != lst)
	{
		(*prv)->NextPtr = (*crr);
		(*crr)->PrevPtr = (*prv);
		(*prv)->Ref = this;
		prv++; crr++;
	}
}

Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::JointPtr 
	Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::FindStartingKnot(  
	_In_ Eifersucht::float32 time, _In_ JointPtr start 
	)
{
	if (start == nullptr) 
	{
		if (Knots.front()->Time() > time) return nullptr;
		return FindStartingKnot(time, Knots.front());
	}
	else if ((start->Time() < time) && (start->NextPtr != nullptr) && (start->NextPtr->Time() <= time)) 
	{
		return FindStartingKnot(time, start->NextPtr);
	} return start;
}

Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::JointPtr 
	Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::FindEndingKnot( 
	_In_ Eifersucht::float32 time, _In_ JointPtr end 
	)
{
	if (end == nullptr) 
	{
		if (Knots.back()->Time() < time) return nullptr;
		else return FindEndingKnot(time, Knots.back());
	}
	else if ((end->Time() >= time) && (end->PrevPtr != nullptr) && (end->PrevPtr->Time() > time)) 
	{
		return FindEndingKnot(time, end->PrevPtr);
	} return end;
}

/*

input : x^3 + a*x^2 + b*x + c = 0

(1) Substitute : y = x + (1/3)a => x = y - (1/3)a to remove quadric term from the polynomial
=> (y - 1/3a)^3 + a(y - 1/3a)^2 + b(y - 1/3a) + c = y^3 - y(b - 1/3a^3) + c - ab/c + 2/27a^3 = 0

(2) Put p = (1/3)a^2 - b
(3) Put q = c + (2/27)a^3 - (ab)/c
=> y^3 + p*y + q = 0

(4) (u + v)^3 = u^3 + 3uv^2 + 3u^2v + v^3 = 3uv(u + v) + (u^3 + v^3)

then y = u + v => 3uv = -p, u^3 + v^3 = -q
then 3uv = -p => 27u^3v^3 = -p^3 => u^3 v^3 = -p^3/27
then put r = u^3, s = v^3

then
rs = (-p/3)^3
r + s = u^3 + v^3 = -q

solve z^2 + qz - (p/3)^3 = 0;

get
u^3 = -(q/2) + sqrt( (q/2)^2 + (p/3)^3 )
v^3 = -(q/2) - sqrt( (q/2)^2 + (p/3)^3 )

pick one u
uv = -p/3 => v = -p/(3u)

y = u + v
x = y - 1/3a

*/
bool Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::Joint::SolveCubicInRadicals(
	_In_ Eifersucht::float32 const a,
	_In_ Eifersucht::float32 const b,
	_In_ Eifersucht::float32 const c,
	_In_ Eifersucht::float32 &x
	)
{
	auto const p = (1.0f/3.0f) * a * a - b;
	auto const q = c + (2.0f/27.0f) * a * a * a - a * b / c;
	auto const d = q * q * (1.0f/4.0f) + p * p * p / (1.0f/27.0f);

	if (d >= 0) // cancel complex roots
	{
		{ // Try first solution u
			auto const u = d - q / 2.0f;
			auto const v = -p / u / 3.0f;
			auto const y = u + v;

			auto const result = y * y * y + p * y + q;
			if (result == 0.0f) 
			{
				x = y - (1.0f/3.0f) * a;
				return true;
			}
		}
		{ // Try the second one
			auto const u = -d - q / 2.0f;
			auto const v = -p / u / 3.0f;
			auto const y = u + v;

			auto const result = y * y * y + p * y + q;
			if (result == 0.0f) 
			{
				x = y - (1.0f/3.0f) * a;
				return true;
			}
		}
	}

	return false;
}

/*

There is another implementation found (written in Mathematica) : 

PositiveCubicRoot[p_, q_, r_] := 
 Module[{po3 = p/3, a, b, det, abs, arg},
  b = ( po3^3 - po3 q/2 + r/2);
  a = (-po3^2 + q/3);
  det = a^3 + b^2;
  If[det >= 0,
   det = Power[Sqrt[det] - b, 1/3];
   -po3 - a/det + det
   ,
   (* evaluate real part, imaginary parts cancel anyway *)
   abs = Sqrt[-a^3];
   arg = ArcCos[-b/abs];
   abs = Power[abs, 1/3];
   abs = (abs - a/abs);
   arg = -po3 + abs*Cos[arg/3]
   ]
  ]
*/

Reign::Vector2 Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::Joint::CubicHermite( 
	_In_ JointPtr const start, 
	_In_ JointPtr const end,
	_In_ Eifersucht::float32 const s
	)
{
	Eifersucht::float32 const s2 = s * s; // s pow 2
	Eifersucht::float32 const s3 = s2 * s; // s pow 3

	Eifersucht::float32 const h[] = 
	{ 
		2 * s3 - 3 * s2 + 1,
		-2 * s3 + 3 * s2,
		s3 - 2 * s2 + s,
		s3 - s2,
	};

	return start->Point * h[0] + end->Point * h[1] + 
		start->Leaves * h[2] + end->Meets * h[3];
}

Eifersucht::float32 Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::CalculateEst(
	_In_ Eifersucht::float32 const time, _In_opt_ JointPtr const hint
	)
{
	JointPtr startingKnot = nullptr;

	if (hint == nullptr) startingKnot = FindStartingKnot(time); 
	else startingKnot = hint;
	if (startingKnot == nullptr) 
	{
		// FindEndingKnot(time);
		startingKnot = Knots.front(); 
		return startingKnot->Point.y;
	}

	if (startingKnot->Time() == time) 
	{
		return startingKnot->Value();
	}

	auto const timeTotal = startingKnot->NextPtr->Time() - startingKnot->Time();
	auto const timeElapsed = time - startingKnot->Time();
	auto const s = timeElapsed / timeTotal;

	auto const pair = CurveCache::Joint::CubicHermite(
		startingKnot, startingKnot->NextPtr, s
		);

	return pair.y;
}

Eifersucht::float32 Eifersucht::TransformationBase::PropertyCache::ChannelCache::CurveCache::Calculate(
	_In_ Eifersucht::float32 const time, _In_opt_ JointPtr const hint
	)
{
	JointPtr startingKnot = nullptr;

	if (hint == nullptr) startingKnot = FindStartingKnot(time); 
	else startingKnot = hint;
	if (startingKnot == nullptr) 
	{
		// FindEndingKnot(time);
		startingKnot = Knots.front(); 
		return startingKnot->Point.y;
	}

	if (startingKnot->Time() == time) 
	{
		return startingKnot->Value();
	}

	auto const a = startingKnot->Time();
	auto const b = startingKnot->NextPtr->Time();
	auto const c = startingKnot->Leaves.x;
	auto const d = startingKnot->NextPtr->Meets.x;

	auto const div = 2 * a - 2 * b + c + d;
	auto const p = (-3 * a + 3 * b - 2 * c) / div;
	auto const q = (c) / div;
	auto const r = a - time;

	auto s = 0.0f;
	if (CurveCache::Joint::SolveCubicInRadicals(p, q, r, s))
	{
		auto const s2 = s * s; // s pow 2
		auto const s3 = s2 * s; // s pow 3

		Eifersucht::float32 const h[] = 
		{ 
			2 * s3 - 3 * s2 + 1,
			-2 * s3 + 3 * s2,
			s3 - 2 * s2 + s,
			s3 - s2,
		};

		return startingKnot->Point.y * h[0] + startingKnot->NextPtr->Point.y * h[1] + 
			startingKnot->Leaves.y * h[2] + startingKnot->NextPtr->Meets.y * h[3];
	}
	else
	{
		return CalculateEst(time, startingKnot);
	}
}
